/**
  ******************************************************************************
  * @file           : chassis_motor_task.h
  * @author         : Tessia
  * @brief          : None
  * @attention      : None
  * @date           : 2024/2/8
  ******************************************************************************
  */



#ifndef SENTRY_CHASSIS_CHASSIS_MOTOR_TASK_H
#define SENTRY_CHASSIS_CHASSIS_MOTOR_TASK_H

#include "DJI_motor_driver.h"
#include "LK_motor_driver.h"
#include "drv_can_dr16.h"
#include "rudder_wheel_kinematic_drv.h"
#include "task_gyro.h"
#include "yaw_task.h"
#include "communicate_task.h"
#include "power_ctrl_drv.h"

const uint32_t LEFT_FRONT_OFFSET = 10461;
const uint32_t LEFT_BACK_OFFSET = 19099;
const uint32_t RIGHT_FRONT_OFFSET = 18090;
const uint32_t RIGHT_BACK_OFFSET = 25413;

//最大速度，同时是开启电容速度
const float SPEED_MAX = 0.9f;
const float OMEGA_MAX = 0.9f;

const float SPEED_SLOPE = 0.5f;
const float OMEGA_ZERO = 0;

const float SPEED_NORMAL = 0.7f;
const float OMEGA_NORMAL = 0.5f;

const float OPTIMAL_REDUNDANCY = PI / 20.0f;


enum motion_type{
    normal = 0,
    gimbal_follow,
    gyroscope,
    auto_navigate
};


#ifdef __cplusplus
extern "C" {
#endif
//C

void chassis_motor_task(void *argument);
float compare_closest(float A,float B,float current);

#ifdef __cplusplus
}
#endif
//C++

#endif //SENTRY_CHASSIS_CHASSIS_MOTOR_TASK_H
